Alles zum Scannen und Plan erstellen

#include "trigono.h"
#include <string.h>

#ifndef bool
#define bool uint8
#endif


#define PLAN_LEER '0'
#define PLAN_MAUER '2'
#define PLAN_LOCH 'L'
#define PLAN_BOT 'B'
#define PLAN_INF 'I'
#define PLAN_ERROR 'E'
#define PLAN_UNKNOWN '1'

#define DIFFWINKEL 5
#define RES 40                 
#define DIM (7 * 240 / RES)    
#define DIM2 (DIM / 2)
#define SENSSIDE 32
#define SENSPOS  35

#define SCHWELLE_MAUER 36
#define SCHWELLE_UNBEKANNT 12
#define SCHWELLE_BOT 900


int16 scanangle;
bool scanning = False;

typedef struct _point
{
    int16 x, y;
} point; 


typedef struct _scandata {
    struct _scandata *next;
    int16 distL, distR;
    int16 lichtL, lichtR;
    int16 winkel;
} scandata;

scandata *firstscan = 0, *lastscan = 0;

typedef struct _plan {
    uint16 lichtwinkel, lichtwert;
    uint8 feld[DIM][DIM];
    uint16 plan[7][7];
} plan;




void __inline__ setPixel(plan *plot, int x, int y, uint8 wert)
{
    x += DIM2;
    y += DIM2;
    if (x < DIM && y < DIM)
    {
        plot->feld[x][y] = wert;
    }
}

uint8 __inline__ getPixel(plan *plot, int x, int y)
{
    x += DIM2;
    y += DIM2;
    if (x < DIM && y < DIM && x >= 0 && y >= 0)
    {
        return plot->feld[x][y];
    } else {
        return PLAN_ERROR;
    }
}


void __inline__ plotLine(plan *plot, int winkel, unsigned int entf, bool r)
{
    // Achtung. Der Winkel, den wir gemessen haben,
    // ist um 90 Grad math. pos. verdreht.
    winkel += 90;
    while (winkel >= 360)
    {
        winkel -= 360;
    }
    while (winkel < 0)
    {
        winkel += 360;
    }

    int Pr_x = r ? floor(SENSSIDE * Sin(winkel) + 0.5) : floor(-SENSSIDE * Sin(winkel) + 0.5);
    int Pr_y = r ? floor(-SENSSIDE * Cos(winkel) + 0.5) : floor(SENSSIDE * Cos(winkel) + 0.5);

    int Pr_X = floor((Pr_x / (float)RES) + 0.5);
    int Pr_Y = floor((Pr_y / (float)RES) + 0.5);

    int PHr_x = Pr_x + floor((SENSPOS + entf) * Cos(winkel) + 0.5);
    int PHr_y = Pr_y + floor((SENSPOS + entf) * Sin(winkel) + 0.5);
    //LOG_INFO(("%d %d %f %f -> %d %d", entf, winkel, Sin(winkel), Cos(winkel), PHr_x, PHr_y));

    int PHr_X = floor((PHr_x / (float)RES) + 0.5);
    int PHr_Y = floor((PHr_y / (float)RES) + 0.5);
    //LOG_INFO(("%d %d %f %f -> %d %d\n", entf, winkel, Sin(winkel), Cos(winkel), PHr_X, PHr_Y));
    int x = Pr_X;
    int y = Pr_Y;
    int sX = (PHr_X < Pr_X ? -1 : 1);
    int dX = abs(PHr_X - Pr_X);
    int sY = (PHr_Y < Pr_Y ? -1 : 1);
    int dY = abs(PHr_Y - Pr_Y); 
    //LOG_INFO(("%d - %d = %d; %d - %d = %d", PHr_X, Pr_X, dX, PHr_Y, Pr_Y, dY));
    //LOG_INFO(("%d %d", sX, sY));
    if (dX >= dY)
    {
        float h = dX / 2.0;
        int i;
        for(i = 0; i <= DIM2; i++)
        {
            int xx = x + i * sX;
            int yy = y;
            //LOG_INFO(("X>=Y: %d %d -> %d %d %d", winkel, entf, xx, yy, (xx < DIM && yy < DIM))); 
            if (i >= dX)
            {
                setPixel(plot, xx, yy, PLAN_MAUER);
            } else {
                setPixel(plot, xx, yy, PLAN_LEER);
            }
            h += dY;
            if (h >= dX)
            {
                h -= dX; 
                y += sY;
            }
        }
    } else {
        float h = dY / 2.0;
        int i;
        for(i = 0; i <= DIM2; i++)
        {
            int xx = x;
            int yy = y + i * sY ;
            //LOG_INFO(("X<Y: %d %d -> %d %d %d", winkel, entf, xx, yy, (xx < DIM && yy < DIM))); 
            if (i >= dY)
            {
                setPixel(plot, xx, yy, PLAN_MAUER);
            } else {
                setPixel(plot, xx, yy, PLAN_LEER);
            }
            h += dX;
            if (h >= dY)
            {
                h -= dY;
                x += sX;
            }
        }
    } 
}


void calcFreeSpace(plan *plot, int winkel, unsigned int left, unsigned right)
{
    plotLine(plot, winkel, left, 0);
    plotLine(plot, winkel, right, 1);
}



void scanPlan(plan *plot)
{
    memset(plot->plan, 0, sizeof(uint16) * 7 * 7);
    int x, y;
    int faktor = 240/RES;
    for(x = -DIM2; x <= DIM2; x++)
    {
        for(y = -DIM2; y <= DIM2; y++)
        {
            uint8 wert = getPixel(plot, x, y);
            if (wert == PLAN_ERROR)
            {
                continue;
            }
            if (wert == PLAN_BOT)
            {
                wert = 100 + '0';
            }
            uint8 xx = (x + DIM2) / faktor;
            uint8 yy = (y + DIM2) / faktor;
            plot->plan[xx][yy] += wert - '0';
        }
    }
    
}



//   <<<<<<<   Main Procedure     >>>>>>>

plan *findPlan(void)
{
    plan *plot = malloc(sizeof(plan));
    memset(plot->feld, PLAN_UNKNOWN, sizeof(plot->feld));
    int minlicht = 2000;
    int minwinkel = 0;
    
    int x, y;
    
    scandata *temp = firstscan;
    while (temp != 0)
    {
        if (temp->lichtL < minlicht)
        {
            minlicht = temp->lichtL;
            minwinkel = temp->winkel;
        }
        calcFreeSpace(plot, temp->winkel, temp->distL, temp->distR);
        temp = temp->next;; 
    }
    plot->lichtwinkel = minwinkel;
    plot->lichtwert = minlicht;
    for(x = -60/RES; x <= 60/RES; x++)
    {
        for(y = -60/RES; y <= 60/RES; y++)
        {
            if (x*x + y*y <= 60*60/(RES*RES))
            {
                setPixel(plot, x, y, PLAN_BOT);
            }
        }
    }
    scanPlan(plot);
    return plot;
}


void freescandata(scandata *first)
{
    while (first != 0)
    {
        scandata *temp = first;
        first = first->next;
        free(temp);
    }
}






// <<<<<  printplan lets you see the plan  >>>>>>>>

void printplan(plan *plot)   
{
    int x, y;
    LOG_INFO(("Licht auf Winkel %d, Wert %d", plot->lichtwinkel, plot->lichtwert));
    LOG_INFO(("Plandimension: %d x %d", DIM, DIM));
    for(y = DIM2; y >= -DIM2; y--)
    {
        char zeile[DIM+1];
        for(x = -DIM2; x <= DIM2; x++)
        {
            zeile[x + DIM2] = getPixel(plot, x, y); 
        }
        zeile[DIM] = 0;
        LOG_INFO(("%s", zeile));
    }
    LOG_INFO(("Lageplan"));
    for(y = 6; y >= 0; y--)
    {
        LOG_INFO(("%03d %03d %03d %03d %03d %03d %03d", plot->plan[0][y], plot->plan[1][y], plot->plan[2][y], plot->plan[3][y], plot->plan[4][y], plot->plan[5][y], plot->plan[6][y]));
    }
}